#!/usr/bin/env python
# encoding: utf-8
from Controller import *
import threading
import SocketServer
import time

flag = False
is_ok = False
step = 5
pitch_angle = 90
yaw_angle = 90

IP_ADDRESS = "192.168.12.1"
PORT = 1234


# socket通信
class MyServer(SocketServer.BaseRequestHandler):
    def handle(self):
        conn = self.request
        conn.sendall(bytes("YunTai>>>Hello, I'm YunTai"))
        mController = Controller()
        mController.reset()
        global pitch_angle, yaw_angle,step
        while flag:
            ret_bytes = conn.recv(64)
            ret_str = str(ret_bytes)
            ret_str2 = ret_str.split()
            for i in ret_str2:
                if i[0:2] == "dx":
                    try:
                        dx = i[2:len(i)]
                        dutyCycleX = float(dx) * 7.5 + 7.5
                        print "dx:",dx, dutyCycleX
                        mController.control_yaw_new(dutyCycleX)
                    except:
                        pass
                elif i[0:2] == "dy":
                    try:
                        dy = i[2:len(i)]
                        dutyCycleY = float(dy) * 6.5 + 6.5
                        print "dy:",dy, dutyCycleY
                        mController.control_pitch_new(dutyCycleY)
                    except:
                        pass
                elif i=="reset":
                    mController.reset()
        conn.sendall(bytes("YunTai>>>Goodbye..."))


def my_socket_server():
    global is_ok
    while not is_ok:
        try:
            server = SocketServer.ThreadingTCPServer((IP_ADDRESS, PORT), MyServer)
            server.serve_forever()
            is_ok = True
        except:
            time.sleep(1)
            print "Not ready yet!"


if __name__ == '__main__':
    try:
        thread1 = threading.Thread(target=my_socket_server)
        thread1.setDaemon(True)
        thread1.start()
        flag = True
        while flag:
            time.sleep(1)
    except (KeyboardInterrupt, SystemExit):
        flag = False
        time.sleep(2)
